#include "mission.h"
#include "imu.h"
#include "stateControl.h"
#include "positionPid.h"
#include <math.h>
#include "stateEstimator.h"
#include "debug.h"

#define ROTATE_RATE 0.05f

typedef struct rotate_s
{
    float angle;
    uint8_t CW;
    uint8_t isOkFlag;
} rotate_t;

typedef struct move_s
{
    axis_t position;
    uint8_t xIsOkFlag;
     uint8_t yIsOkFlag;
    uint8_t zIsOkFlag;
} move_t;

static rotate_t rotate;
static move_t move;
static uint8_t calibrationIsOkFlag;
static uint8_t takeOffIsOkFlag;
static uint8_t landIsOkFlag;
static uint8_t velIsOkFlag;

//值变化代表执行完成
uint8_t mission = 1;

void missionInit(void)
{
    rotate.isOkFlag = 1;
    move.xIsOkFlag = 1;
     move.yIsOkFlag = 1;
    move.zIsOkFlag = 1;
    calibrationIsOkFlag = 1;
    takeOffIsOkFlag = 1;	
    landIsOkFlag = 1;	
	velIsOkFlag=1;
}							
							
void missionUpdate(float dt)
{
    static float sumDt = 0;
    sumDt += dt;
    // 控制偏航的需要快点
    if (0 == rotate.isOkFlag) 
    {			
        if (fabs(rotate.angle) > 0.3f)
        {		
            float angle = (rotate.CW == 1) ? ROTATE_RATE : -ROTATE_RATE;
            setDesiredYawAddAngle(angle);				
            rotate.angle -= angle;								
        }								
        else							
        {								
            rotate.isOkFlag = 1;		
            mission++;					
        }								
    }									
    // 1秒刷新一次，判断任务是否执行完成
    if (sumDt > 1.0f)
    {
        if (0 == move.xIsOkFlag)
        {
            if ((fabs(getErrorPosX())<5)&&(fabs(getErrorPosY())<5))
            {
                move.xIsOkFlag = 1; 
                mission++;
            }
        }
        if (0 == move.yIsOkFlag)
        {
            //if (fabs(getErrorPosY()) < 5)
			if ( (fabs(getErrorPosX())<5)&& (fabs(getErrorPosY())<5) )
            {
                move.yIsOkFlag = 1;
                  mission++;
            }
        }

        if (0 == move.zIsOkFlag)
        {
            if (fabs(getErrorPosZ()) < 5)//必须要实时相减的值
            {
                move.zIsOkFlag = 1;  
                mission++;
            }
        }
        if (0 == calibrationIsOkFlag)
        {
            if (!isCalibrating())
            {
                calibrationIsOkFlag = 1;
                mission++;
            }
        }
        if (0 == takeOffIsOkFlag)
        {
            if ((fabs(getErrorPosZ()) < 5)&&(g_pStaEstimator->pos[Z]>50)&&(getFlightState()!=CONTROL_MODE_TAKEOFF))
            {
                takeOffIsOkFlag = 1;
                mission++;
            }
        }
        if (0 == landIsOkFlag)
        {
            if (getFlightState() == CONTROL_MODE_STOP)
            {
                landIsOkFlag = 1;
                mission++;
            }
        }
		
		if(0 == velIsOkFlag)
		{
			velIsOkFlag= 1;
            mission++;
		}
    }
    if (mission == 0)
    {
         mission = 1;
    }
	
	DEBUG[3]=move.zIsOkFlag*10;
	DEBUG[6]=takeOffIsOkFlag*10; 
//	DEBUG[4]=getErrorPos(Z);
	DEBUG[5]=mission*10;
}

void missionRotate(float CW, float angle) 
{
    rotate.angle = angle;
    rotate.CW = CW;
    rotate.isOkFlag = 0;
}
void setVelIsOkFlag(uint8_t number)
{
	velIsOkFlag=number;
}
void missionMove(float x, float y, float z, uint8_t enableBit)
{
    getTargetPos(&move.position);
	float xtemp,ytemp;
    if ((enableBit & (1 << X)) != 0)
    {	 
		xtemp=  x*yawTrigo.cosYaw-yawTrigo.sinYaw*y;
		ytemp=  x*yawTrigo.sinYaw+yawTrigo.cosYaw*y;
		
		move.position.x += xtemp;
		move.position.y += ytemp;
		setTargetPos(move.position, ((1 << X)|(1 << Y)) );// 
        move.xIsOkFlag = 0;
    }
    if ((enableBit & (1 << Y)) != 0)
    {
		xtemp=  x*yawTrigo.cosYaw-yawTrigo.sinYaw*y;
		ytemp=  x*yawTrigo.sinYaw+yawTrigo.cosYaw*y;
		
		move.position.x += xtemp;
		move.position.y += ytemp;
		setTargetPos(move.position, ((1 << X)|(1 << Y)) );// 
        move.yIsOkFlag = 0;
    }
    if ((enableBit & (1 << Z)) != 0)
    {
        move.position.z += z; 
		setTargetPos(move.position, enableBit);// 
        move.zIsOkFlag = 0;
    }
//	move.position.x= move.position.x*yawTrigo.cosYaw - yawTrigo.sinYaw*move.position.y;
//	move.position.y= move.position.x*yawTrigo.sinYaw + yawTrigo.cosYaw*move.position.y;
//	setTargetPos(move.position, enableBit);//试试这个放上面
}

void missionCalibration(void)
{
    calibrationIsOkFlag = 0;
    if (getFlightState() == CONTROL_MODE_STOP)
    {
        setFlightState(CONTROL_MODE_CALIBRAT);
    }
    else
    {
        // 失败则直接任务加1
        missionAdd();
    }
}

void missionTakeOff(void)
{
    takeOffIsOkFlag = 0;
    setArmFlag(1);
    // if (g_pRemoteControl->thrust > 900)
    // {
    //     setFlightState(CONTROL_MODE_TEST_MOTOR);
    // }
    // else
    {
        setFlightState(CONTROL_MODE_TAKEOFF);
    }
}

void missionLand(void)
{
    landIsOkFlag = 0;
    setFlightState(CONTROL_MODE_LAND);
}

uint8_t getMissionCount(void)
{
    return mission;
}

void missionAdd(void)
{
   mission++;
    if (mission == 0)
    {
        mission = 1;
    }
}
float getErrorYaw(void)
{
	return rotate.angle;
}
